#ifndef ENTITYTEMPLATES_H
#define ENTITYTEMPLATES_H
#include <irrlicht.h>
#include "IVehicle.h"
#include "GameObject.h"
using namespace irr;
using namespace core;

template <class T, class conT>
void EnforceNonPenetrationContraint(T entity, const conT& others)
{
	typename conT::const_iterator it;

	//iterate through all entities checking for any overlap of bounding
	//radii
	for (it=others.begin(); it != others.end(); ++it)
	{
		//make sure we don't check against this entity
		if (*it == entity) continue;

		//calculate the distance between the positions of the entities
		core::vector3df ToEntity = entity->getPosition() - (*it)->getPosition();

		double DistFromEachOther = ToEntity.getLength();

		//if this distance is smaller than the sum of their radii then this
		//entity must be moved away in the direction parallel to the
		//ToEntity vector   
		double AmountOfOverLap = (*it)->BRadius() + entity->BRadius() -
			DistFromEachOther;

		if (AmountOfOverLap >= 0)
		{
			//move the entity a distance away equivalent to the amount of overlap.
			entity->setPosition(entity->getPosition() + (ToEntity/DistFromEachOther) *
				AmountOfOverLap);
		}
	}//next entity
}

#endif ENTITYTEMPLATES_H